import sys 
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
import argparse
import time

from algorithm.CarControl import CarControl


car = CarControl("/dev/ttyS0")

def car_open_move_for_distance(sleep_time, speed=0.1):
    car.setSpeed(speed, 0)
    time.sleep(sleep_time)
    car.setSpeed(0, 0)

def car_open_rotate_for_angle(sleep_time, dspeed=30):
    car.setSpeed(0, dspeed)
    time.sleep(sleep_time)
    car.setSpeed(0, 0)

def car_imu_test():
    """
    test imu
    """
    print(car.getMPU())

def car_setMessage(line, message):
    """
    set message
    """
    car.setMessage(line, message)

def car_swget():
    """
    get switch status
    """
    sw = car.getSwStatus()
    print(f"switch status: {sw}")
    return sw

def main():
    """
    main function
    """
    parser = argparse.ArgumentParser(description="car move test")
    parser.add_argument("--task", type=str, default="car_imu_test", help="task name")
    parser.add_argument("--time", type=float, default=10, help="time to move")
    parser.add_argument("--line", type=int, default=0, help="line to set message")
    parser.add_argument("--message", type=str, default="hello world", help="message to set")
    parser.add_argument("--speed", type=float, default=0.1, help="speed to move")
    parser.add_argument("--dspeed", type=float, default=30, help="dspeed to rotate")
    args = parser.parse_args()
    task_name = args.task

    if task_name == "car_open_move_for_distance":
        car_open_move_for_distance(args.time, args.speed)
    elif task_name == "car_open_rotate_for_angle":
        car_open_rotate_for_angle(args.time, args.dspeed)
    elif task_name == "car_imu_test":
        car_imu_test()
    elif task_name == "car_setMessage":
        car_setMessage(args.line, args.message)
    elif task_name == "car_swget":
        car_swget()
    else:
        print("task name error")


if __name__ == "__main__":
    main()